﻿/*
 * This file is part of MonoStrategy.
 *
 * Copyright (C) 2010-2011 Christoph Husse
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU Affero General Public License as
 *  published by the Free Software Foundation, either version 3 of the
 *  License, or (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU Affero General Public License for more details.
 *
 *  You should have received a copy of the GNU Affero General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 * Authors: 
 *      # Christoph Husse
 * 
 * Also checkout our homepage: http://monostrategy.codeplex.com/
 */
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Runtime.Serialization;
using System.Runtime.Serialization.Formatters.Binary;
using System.IO;

namespace MonoStrategy
{
    /// <summary>
    /// In constrast to user controlables, which are currently not supported, AI controlables need 
    /// to be planned under strong constraints, since the user has almost no chance to correct
    /// misplanned paths or even cycle loops. The following class will make strong path planning
    /// more efficient by providing precomputed routes through the map. So instead of having to
    /// calculate the entire path, a movable only needs to compute the path to the next global 
    /// path node and then follow the precomputed route to the target global path node. Now again
    /// another short path is being computed to the precise target. Later, this it will be possible
    /// to implement some sort of easy and fast cooperative path planning along these routes while
    /// using the same technique as used for user controlables (which is not implemented yet) to
    /// schedule the cooperative paths from and to the global path route. Both techniques together
    /// will allow to handle even 60.000 units in a cooperative and solid way maybe even on embedded
    /// devices. Later, the routing should also be updated when terrain changes through priest spells
    /// for example. For now only buildings and foilage are taken into account as dynamic parameter.
    /// </summary>
    [Serializable]
    internal partial class TerrainRouting
    {
        private byte[,] m_Tiling;
        private int m_SizeInCells;
        private int m_SizeInNodes;
        private const Int32 GranularityShift = 4;
        private const Int32 Granularity = (1 << GranularityShift);
        private RoutingNode[][] m_RoutingTable;
        private readonly RoutingNode[] m_TmpBufferSrc = new RoutingNode[4];
        private readonly RoutingNode[] m_TmpBufferDst = new RoutingNode[4];
        [NonSerialized]
        private CooperativeAStar m_CoopAStar;

        internal Int64 CurrentCycle { get; private set; }
        internal Int64 CycleResolution { get; private set; }
        internal int Size { get { return m_SizeInCells; } }
        public TerrainDefinition Terrain { get; private set; }

        [OnDeserialized]
        private void OnDeserialized(StreamingContext ctx)
        {
            m_CoopAStar = new CooperativeAStar(this);
        }

        [Serializable]
        private enum RoutingDirection
        {
            _000,
            _045,
            _090,
            _135,
            _180,
            _225,
            _270,
            _315,
        }

        [Serializable]
        private class RoutingNodeLink
        {
            public readonly RoutingNode Node;
            public Double Costs { get; private set; }

            public RoutingNodeLink(RoutingNode inNode, Double inCosts)
            {
                if (inNode == null)
                    throw new ArgumentNullException();

                Node = inNode;
                Costs = inCosts;
            }
        }

        /// <summary>
        /// Routing nodes represent a network graph which provides a higher granulated level
        /// 
        /// </summary>
        [Serializable]
        private class RoutingNode : IQueueItem
        {
            /// <summary>
            /// There are eight neighbors starting from 12:00 clockwise in 45 degree steps.
            /// If there is no such neighbor for a given direction, the entry is NULL.
            /// </summary>
            public readonly RoutingNodeLink[] Neighbors = new RoutingNodeLink[8];
            public readonly Single[][] Heuristics;
            public readonly Int32 NodeX;
            public readonly Int32 NodeY;
            public readonly Point CellXY;
            public readonly Point NodeXY;
            public double F { get; set; }
            public double G;
            public double H;
            public int Index { get; set; }

            public RoutingNode(int inSizeInNodes, int inX, int inY)
            {
                Heuristics = new Single[inSizeInNodes][];

                for (int i = 0; i < inSizeInNodes; i++)
                {
                    Heuristics[i] = new Single[inSizeInNodes];
                }

                NodeX = inX >> GranularityShift;
                NodeY = inY >> GranularityShift;
                NodeXY = new Point(NodeX, NodeY);
                CellXY = new Point(inX, inY);
            }
        }


        internal TerrainRouting(TerrainDefinition inTerrain, Int64 inCurrentCycle, Int64 inCycleResolution)
        {
            Terrain = inTerrain;
            CycleResolution = inCycleResolution;
            CurrentCycle = inCurrentCycle;
            m_SizeInCells = inTerrain.Size;
            m_SizeInNodes = inTerrain.Size >> GranularityShift;
            m_CoopAStar = new CooperativeAStar(this);
        }

        internal void AddOneCycle()
        {
            CurrentCycle++;
        }

        public void Analyze()
        {
            RoutingNodeAStar routingNodeAStar = new RoutingNodeAStar(this);
            AStarSolver backwardAStar = new AStarSolver(Terrain);

            if ((m_SizeInCells % Granularity) != 0)
                throw new ApplicationException();

            m_Tiling = new byte[m_SizeInCells, m_SizeInCells];

            // put all nodes into the open list
            Stack<Point> openList = new Stack<Point>();

            for (int x = 0; x < m_SizeInCells; x++)
            {
                for (int y = 0; y < m_SizeInCells; y++)
                {
                    openList.Push(new Point(x, y));
                }
            }

            // compute a map of unconnected map tiles, providing a constant time mapping between a 
            // map position and the tile it belongs to.
            int tileIndex = 1;
            List<Point> tileCache = new List<Point>();

            while (openList.Count > 0)
            {
                Point node = openList.Pop();

                if (m_Tiling[node.X, node.Y] != 0)
                    continue; // node is already assigned to a map tile

                if (Terrain.GetWallAt(node.X, node.Y) == WallValue.WaterBorder)
                    continue; // water borders are not threaded as walkable for anything, so neither threaded as tile

                // tile can NOT belong to any existing tile, otherwise A-Star would have found the connection in first place.
                if (tileIndex > 255)
                    throw new NotSupportedException("Map contains more than 255 unconnected tiles.");

                // enumerate all nodes reachable from this map node
                tileCache.Clear();
                backwardAStar.ComputeConnectedTileAround(node, (int)WallValue.Building, tileCache);

                foreach (var entry in tileCache)
                {
                    m_Tiling[entry.X, entry.Y] = (byte)tileIndex;
                }

                tileIndex++;
            }

            tileCache.Clear();

            /* Create the routine node table by computing neighbors and their a-star distances for/to each
             * routing node.
             */

            // since all computation attempts are guaranteed to succeed, A-Star operates resonably fast here!
            int sizeInNodes = m_SizeInCells / Granularity;
            RoutingDirection[] directions = new RoutingDirection[] { 
                RoutingDirection._315, RoutingDirection._270, RoutingDirection._225,
                RoutingDirection._000, RoutingDirection._180,
                RoutingDirection._045, RoutingDirection._090, RoutingDirection._135,
            };

            m_RoutingTable = new RoutingNode[sizeInNodes][];

            for (int i = 0; i < sizeInNodes; i++)
            {
                m_RoutingTable[i] = new RoutingNode[sizeInNodes];
            }

            for (int xSrc = 0; xSrc < m_SizeInCells; xSrc += Granularity)
            {
                for (int ySrc = 0; ySrc < m_SizeInCells; ySrc += Granularity)
                {
                    if (Terrain.IsWater(new Point(xSrc, ySrc)))
                        continue;

                    RoutingNode srcNode = m_RoutingTable[xSrc / Granularity][ySrc / Granularity];

                    if (srcNode == null)
                        m_RoutingTable[xSrc / Granularity][ySrc / Granularity] = srcNode = new RoutingNode(sizeInNodes, xSrc, ySrc);

                    // compute distance to neighbors
                    // ATTENTION: if you change these loops, you may have to adjust "directions" properly!
                    for (int xDst = xSrc - Granularity, offset = 0; xDst < xSrc + 2 * Granularity; xDst += Granularity)
                    {
                        for (int yDst = ySrc - Granularity; yDst < ySrc + 2 * Granularity; yDst += Granularity)
                        {
                            if ((xDst < 0) || (yDst < 0) || (xDst >= m_SizeInCells) || (yDst >= m_SizeInCells))
                                continue;

                            Point src = new Point(xSrc, ySrc);
                            Point dst = new Point(xDst, yDst);

                            if ((src == dst) || !AreConnected(src, dst) || Terrain.IsWater(dst))
                                continue;

                            /* Computing a path to a node neighbour may seem akward, but in fact such paths
                             * can get very long on ugly shaped maps. And later, the entire heuristic will be
                             * computed based on these distances between nodes we are computing now and thus
                             * they have to be as accurate as possible.
                             */
                            RoutingNode dstNode = m_RoutingTable[xDst / Granularity][yDst / Granularity];
                            Double pathCosts;

                            if (dstNode == null)
                                m_RoutingTable[xDst / Granularity][yDst / Granularity] = dstNode = new RoutingNode(sizeInNodes, xDst, yDst);

                            backwardAStar.BeginSearch(src, dst, (int)WallValue.Building);
                            pathCosts = backwardAStar.GetClosedGValue(dst);

                            if (pathCosts < 0)
                                throw new ApplicationException("Nodes are connected but path costs are negative!");

                            srcNode.Neighbors[(int)directions[offset++]] = new RoutingNodeLink(dstNode, pathCosts);
                        }
                    }
                }
            }

            /* A node surrounds a cell if it is within a "Granularity - 1" radius (manhattan metric) 
             * around the cell AND is reachable from that cell. This limits the maximum count of
             * surroundings to four.
             * 
             * If a cell has no surroundings, the map is INVALID and will raise an exception!
             * For example a small island within a block of four nodes would produce this error.
             */
            RoutingNode[] surroundings = new RoutingNode[4];

            for (int x = 0; x < m_SizeInCells; x++)
            {
                for (int y = 0; y < m_SizeInCells; y++)
                {
                    Point cell = new Point(x, y);

                    if (!AreConnected(cell, cell) || Terrain.IsWater(cell))
                        continue; // this won't work for water (border) cells

                    GetSurroundings(cell, surroundings);

                    if (surroundings.Count(e => e != null) == 0)
                        throw new ArgumentException("Invalid map! Each cell shall have at least one surrounding.");
                }
            }

            /*
             * Compute granulated heuristics between all non-water nodes. This implies again a wide 
             * range of maps being tagged invalid, since all connected land node must be heuristicly
             * connected, which is not the case for all ugly shaped maps. Further even if this succeeds,
             * the heuristic distance might be far off an optimal path when the map is not well shaped...
             */
            for (int xSrc = 0; xSrc < m_SizeInNodes; xSrc++)
            {
                for (int ySrc = 0; ySrc < m_SizeInNodes; ySrc++)
                {
                    RoutingNode srcNode = m_RoutingTable[xSrc][ySrc];

                    if (srcNode == null)
                        continue;

                    routingNodeAStar.Initialize(srcNode);

                    for (int xDst = 0; xDst < m_SizeInNodes; xDst++)
                    {
                        for (int yDst = 0; yDst < m_SizeInNodes; yDst++)
                        {
                            RoutingNode dstNode = m_RoutingTable[xDst][yDst];

                            if (dstNode == null)
                            {
                                srcNode.Heuristics[xDst][yDst] = Single.NaN;

                                continue;
                            }

                            if (!AreConnected(srcNode.CellXY, dstNode.CellXY))
                                continue;

                            // distance between distinct nodes can never be zero, so it's just not computed yet
                            Double heuristic;

                            if (!routingNodeAStar.GetHeuristic(dstNode, out heuristic))
                                throw new ArgumentException("Map is INVALID! No heuristic connection between [" + srcNode.CellXY + "] and [" + dstNode.CellXY + "]");

                            srcNode.Heuristics[dstNode.NodeX][dstNode.NodeY] = (Single)heuristic;
                        }
                    }
                }
            }

            // all other data is computed on the fly and cached...

            /* Before we can start using this concept, we need to proof that connections on
             * cell level are equal to connections on a heuristic level, otherwise we might
             * run into ugly bugs later on...
             * 
             * "=>": If two cells are connected, then so they are heuristicly connected.
             *      # By precondition, each cells must have at least one surrounding
             *      # So let's assume the source and target node are NOT connected but
             *        the cells are
             *      # Since both cells are connected to their surroundings by definition,
             *        and connected to each other, it follows that their surrounding must
             *        be connected as well (since we can now construct an explicit path).
             *      
             * "<=": If two cells are heuristicly connected, then so they are connected.
             *      # Follows constructively by using the paths from the cells to their
             *        surroundings and by precondition, the paths between these surroundings.
             *        
             * So whats the sense about proving this more or less obvious assumption? Well
             * its not that obvious actually and also we've seen that the far-fetched 
             * restrictions to reject maps with unsurrounded cells, is a requirement to
             * proof this assumption. But of course we didn't proof that we couldn't proof it
             * without this restrictions, but I guess things start to get messy when we try 
             * to, also heavily reducing the speed of heuristic lookups. So even if the
             * restriction might not be a mathematical requirement, it seems to be a 
             * requirement in terms of speed.
             */
        }

        public void SaveToStream(Stream inStream)
        {
            BinaryFormatter format = new BinaryFormatter();

            format.Serialize(inStream, this);
        }

        public void LoadFromStream(Stream inStream)
        {
            BinaryFormatter format = new BinaryFormatter();
            TerrainRouting source = (TerrainRouting)format.Deserialize(inStream);

            if (source.Size != Size)
                throw new ArgumentException("The routing data stored in the stream is not compatible with this instance.");

            this.m_RoutingTable = source.m_RoutingTable;
            this.m_Tiling = source.m_Tiling;
        }

        /// <summary>
        /// Checks wether a path could be found between two nodes, if attempted. This method
        /// executes in constant time and should always be checked before a path search is
        /// invoked. Failed path searches are computational extremely expensive.
        /// </summary>
        public bool AreConnected(Point inSource, Point inTarget)
        {
            return (m_Tiling[inSource.X, inSource.Y] == m_Tiling[inTarget.X, inTarget.Y]) && (m_Tiling[inSource.X, inSource.Y] != 0);
        }

        private void GetSurroundings(Point inCell, RoutingNode[] outSurroundings)
        {
            int x = inCell.X >> GranularityShift;
            int y = inCell.Y >> GranularityShift;

            outSurroundings[0] = m_RoutingTable[x][y];

            if ((inCell.X == (x << GranularityShift)) && (inCell.Y == (y << GranularityShift)))
            {
                outSurroundings[1] = null;
                outSurroundings[2] = null;
                outSurroundings[3] = null;
            }
            else
            {
                outSurroundings[1] = (x + 1 < m_SizeInNodes) ? m_RoutingTable[x + 1][y] : null;
                outSurroundings[2] = ((x + 1 < m_SizeInNodes) && (y + 1 < m_SizeInNodes)) ? m_RoutingTable[x + 1][y + 1] : null;
                outSurroundings[3] = (y + 1 < m_SizeInNodes) ? m_RoutingTable[x][y + 1] : null;
            }
        }

        private double GetHeuristic(Point inSrc, Point inDst)
        {
            RoutingNode minSrcNode = null, minDstNode = null;
            Double minPathCosts = Double.PositiveInfinity;

            GetSurroundings(inSrc, m_TmpBufferSrc);
            GetSurroundings(inDst, m_TmpBufferDst);

            for (int iSrc = 0; iSrc < 4; iSrc++)
            {
                var srcSurrounding = m_TmpBufferSrc[iSrc];

                if(srcSurrounding == null) 
                    continue;

                double pathCosts_Src = inSrc.DistanceTo(srcSurrounding.CellXY);

                for (int iDst = 0; iDst < 4; iDst++)
                {
                    var dstSurrounding = m_TmpBufferDst[iDst];

                    if ((dstSurrounding == null) || !AreConnected(srcSurrounding.CellXY, dstSurrounding.CellXY))
                        continue;

                    double pathCosts = pathCosts_Src + inDst.DistanceTo(dstSurrounding.CellXY) + srcSurrounding.Heuristics[dstSurrounding.NodeX][dstSurrounding.NodeY];

                    if (pathCosts < minPathCosts)
                    {
                        minPathCosts = pathCosts;
                        minDstNode = dstSurrounding;
                        minSrcNode = srcSurrounding;
                    }
                }
            }

            if (minDstNode != null)
            {
                if (minDstNode.CellXY == minSrcNode.CellXY)
                {
                    return inSrc.DistanceTo(inDst);
                }
            }
            else
                return Double.NaN;

            return minPathCosts;
        }

        public void ClearPath(Movable inMovable)
        {
            m_CoopAStar.UnregisterPath(inMovable.Path);
            inMovable.Path.Clear();
        }
        
        public void SetDynamicPath(
            Movable inMovable,
            Point inEnd,
            MovableType inMovableType)
        {
            if ((inMovable.CycleSpeed < 1) || (inMovable.CycleSpeed > 1000))
                throw new ArgumentOutOfRangeException();

            Debug.Assert((CurrentCycle % CycleResolution) == 0);

            // search for current path position and remove outdated path nodes
            LinkedListNode<MovablePathNode> pathNodes = inMovable.Path.Last;
            LinkedListNode<MovablePathNode> currentPathNode = null;

            while (pathNodes != null)
            {
                LinkedListNode<MovablePathNode> prev = pathNodes.Previous;

                if (pathNodes.Value.Time != CurrentCycle)
                {
                    m_CoopAStar.UnregisterPathNode(pathNodes.Value);
                    inMovable.Path.Remove(pathNodes);
                }
                else
                    currentPathNode = pathNodes;

                pathNodes = prev;
            }

            Debug.Assert(currentPathNode != null, "Movable path does not contain a path node with current cycle time.");

            var startNode = currentPathNode.Value;

            ClearPath(inMovable);

            if (!AreConnected(startNode.Position, inEnd))
            {
                XMLTerrainGenerator.Save(Terrain.m_WallMap);

                throw new ArgumentException();
            }


            m_CoopAStar.Search(inMovable, new PathNodeKey(startNode.Position.X, startNode.Position.Y, 0), new PathNodeKey(inEnd.X, inEnd.Y, 0), WallValue.Reserved);

            inMovable.CurrentNode = inMovable.Path.First;
            inMovable.IsInvalidated = false;
        }
    }
}
